#include "Precompile.h"
#include "MatrixCamera.h"

namespace Utility{
	MatrixCamera::MatrixCamera():ICamera(){
	    m_maxPitch        = 89.0f * E_PI/180.0f;
	    m_maxVelocity     = 1.0f;
	    m_invertY         = false;
	    m_enableYMovement = true;
		m_look(Math::_z_,1.0f);

	    update(0);
	}

	void MatrixCamera::moveForward( float units ){
	    if ( m_enableYMovement ){
	    	m_velocity += m_look * units;
	    }
	    else{
			Math::Vector<float> moveVector;
			moveVector(Math::_x_,m_look(Math::_x_));
			moveVector(Math::_z_,m_look(Math::_z_));
			moveVector.normalize();

	        moveVector *= units;
	        m_velocity += moveVector;
	    }
	}

	void MatrixCamera::strafe( float units ){
	    m_velocity += m_right * units;
	}

	void MatrixCamera::moveUp( float units ){
	    if ( m_enableYMovement ){
			m_velocity(Math::_y_,m_velocity(Math::_y_)+units);
	    }
	}

	void MatrixCamera::yaw( float radians ){
	    if ( radians == 0.0f ){
	        return;
	    }

		Math::Matrix<4,4> rot;
		rot.createFromAxisAngle(m_up,radians);
		rot.transform(m_right);
		rot.transform(m_look);
		m_right.normalize();
		m_look.normalize();
	}

	void MatrixCamera::pitch( float radians ){
	    if ( radians == 0.0f ){
	        return;
	    }

	    radians = (m_invertY) ? -radians : radians;
	    m_pitch -= radians;
	    if ( m_pitch > m_maxPitch ){
	        radians += m_pitch - m_maxPitch;
	    }
	    else if ( m_pitch < -m_maxPitch ){
	        radians += m_pitch + m_maxPitch;
	    }

		Math::Matrix<4,4> rot;
		rot.createFromAxisAngle(m_right,radians);
		rot.transform(m_up);
		rot.transform(m_look);
		m_up.normalize();
		m_look.normalize();
	}

	void MatrixCamera::roll( float radians ){
	    if ( radians == 0.0f ){
	        return;
	    }

		Math::Matrix<4,4> rot;
		rot.createFromAxisAngle(m_look,radians);
		rot.transform(m_right);
		rot.transform(m_up);
		m_right.normalize();
		m_up.normalize();
	}

	void MatrixCamera::update(float dt){
	    // Cap velocity to max velocity
		if ( m_velocity.length() > m_maxVelocity ){
			m_velocity.normalize();
	        m_velocity *= m_maxVelocity;
	    }

	    // Move the camera
	    m_position += m_velocity;

		// Speed approximation
		m_distTraveled += m_velocity.length();
		if(m_velocityTimer.elapsed()>0.2){
			m_speed = int(m_distTraveled*(1.0f/m_velocityTimer.elapsed())*3.6f);
			m_distTraveled = 0;
			m_velocityTimer.restart();
		}

	    // Could decelerate here. I'll just stop completely.
	    m_velocity *= 10*dt;
	    m_lookAt = m_position + m_look;
	    
	    // Calculate the new view matrix
		D3DXMATRIX view;
	    D3DXVECTOR3 up = D3DXVECTOR3( 0.0f, 1.0f, 0.0f );
		D3DXVECTOR3 lookAt;
		lookAt.x = m_lookAt(Math::_x_);
		lookAt.y = m_lookAt(Math::_y_);
		lookAt.z = m_lookAt(Math::_z_);
		D3DXVECTOR3 position;
		position.x = m_position(Math::_x_);
		position.y = m_position(Math::_y_);
		position.z = m_position(Math::_z_);
	    D3DXMatrixLookAtLH( &view, &position, &lookAt, &up );

		memcpy(m_viewMatrix.m_data,view.m,sizeof(float)*16);

	    // Set the camera axes from the view matrix
		m_right(Math::_x_,m_viewMatrix.get(0,0));
		m_right(Math::_y_,m_viewMatrix.get(1,0));
		m_right(Math::_z_,m_viewMatrix.get(2,0));
		m_up(Math::_x_,m_viewMatrix.get(0,1));
		m_up(Math::_y_,m_viewMatrix.get(1,1));
		m_up(Math::_z_,m_viewMatrix.get(2,1));
		m_look(Math::_x_,m_viewMatrix.get(0,2));
		m_look(Math::_y_,m_viewMatrix.get(1,2));
		m_look(Math::_z_,m_viewMatrix.get(2,2));

	    // Calculate yaw and pitch
	    float lookLengthOnXZ = sqrtf( m_look(Math::_z_) * m_look(Math::_z_) + m_look(Math::_x_) * m_look(Math::_x_) );
		m_pitch = atan2f( m_look(Math::_y_), lookLengthOnXZ );
		m_yaw   = atan2f( m_look(Math::_x_), m_look(Math::_z_) );

		//D3DXMATRIX viewProj = m_view*m_projection;
		m_viewProjMatrix = m_viewMatrix * m_projectionMatrix;
	}

	void MatrixCamera::setPosition( Math::Vector<float>* pPosition ){
		m_position = *pPosition;
	}

	void MatrixCamera::setLookAt( Math::Vector<float>* pLookAt ){
		m_lookAt = *pLookAt;
		m_lookAt -= m_position;
		m_lookAt.normalize();
	    //D3DXVec3Normalize( &m_look, &(m_lookAt - position) );
	}

	Math::Matrix<4,4>& MatrixCamera::getViewProjection(){
		return m_viewProjMatrix;
	}

	void MatrixCamera::updateViewMatrix(){
		//D3DXVECTOR3 position;
		//position.x = m_position(Utility::Math::_x_);
		//position.y = m_position(Utility::Math::_y_);
		//position.z = m_position(Utility::Math::_z_);

		//// Calculate the new view matrix
	 //   D3DXVECTOR3 up = D3DXVECTOR3( 0.0f, 1.0f, 0.0f );
	 //   D3DXMatrixLookAtLH( &m_view, &position, &m_lookAt, &up );

	 //   // Set the camera axes from the view matrix
	 //   m_right.x = m_view._11;  
	 //   m_right.y = m_view._21;  
	 //   m_right.z = m_view._31;  
	 //   m_up.x = m_view._12;
	 //   m_up.y = m_view._22;
	 //   m_up.z = m_view._32;
	 //   m_look.x = m_view._13;
	 //   m_look.y = m_view._23;
	 //   m_look.z = m_view._33;
	}
}